# -*- coding: utf-8 -*-
"""
升级版目标跟踪器 - 集成UKF和IMM算法
包含无迹卡尔曼滤波、多模型交互、自适应参数调整等先进功能
"""

import numpy as np
import cv2
from typing import List, Tuple, Dict, Optional
import time
from scipy.optimize import linear_sum_assignment
from scipy.spatial.distance import mahalanobis


class UnscentedKalmanFilter:
    """
    无迹卡尔曼滤波器 - 比EKF更快更准的非线性滤波器
    状态向量: [x, y, vx, vy, ax, ay] - 位置、速度、加速度
    """
    
    def __init__(self, dt=1.0/30.0, alpha=1e-3, beta=2.0, kappa=0.0):
        """
        初始化UKF
        Args:
            dt: 时间步长
            alpha: Sigma点分布参数
            beta: 分布类型参数 (高斯分布=2)  
            kappa: 次要缩放参数
        """
        self.dt = dt
        self.dim_x = 6  # 状态维度 [x, y, vx, vy, ax, ay]
        self.dim_z = 2  # 观测维度 [x, y]
        
        # UKF参数
        self.alpha = alpha
        self.beta = beta
        self.kappa = kappa
        self.lambda_ = alpha**2 * (self.dim_x + kappa) - self.dim_x
        
        # 状态向量 [x, y, vx, vy, ax, ay]
        self.x = np.zeros((self.dim_x, 1))
        
        # 状态协方差矩阵
        self.P = np.eye(self.dim_x) * 100.0
        
        # 过程噪声协方差矩阵 (优化后的噪声模型)
        self.Q = np.diag([0.1, 0.1, 1.0, 1.0, 2.0, 2.0])  # 位置、速度、加速度噪声
        
        # 观测噪声协方差矩阵
        self.R = np.eye(self.dim_z) * 5.0
        
        # Sigma点权重
        self.n_sigma = 2 * self.dim_x + 1
        self.Wm = np.zeros(self.n_sigma)  # 均值权重
        self.Wc = np.zeros(self.n_sigma)  # 协方差权重
        
        self.Wm[0] = self.lambda_ / (self.dim_x + self.lambda_)
        self.Wc[0] = self.lambda_ / (self.dim_x + self.lambda_) + (1 - alpha**2 + beta)
        for i in range(1, self.n_sigma):
            self.Wm[i] = self.Wc[i] = 0.5 / (self.dim_x + self.lambda_)
            
        # 初始化标志
        self.initialized = False
        
        # 性能优化：预分配数组
        self.sigma_points = np.zeros((self.dim_x, self.n_sigma))
        self.sigma_h = np.zeros((self.dim_z, self.n_sigma))
        
    def initialize(self, measurement):
        """使用第一次观测初始化滤波器"""
        self.x[0, 0] = measurement[0]  # x
        self.x[1, 0] = measurement[1]  # y  
        self.x[2, 0] = 0.0  # vx
        self.x[3, 0] = 0.0  # vy
        self.x[4, 0] = 0.0  # ax
        self.x[5, 0] = 0.0  # ay
        self.initialized = True
        
    def _generate_sigma_points(self, x, P):
        """生成Sigma点 - 优化版本"""
        try:
            sqrt = np.linalg.cholesky((self.dim_x + self.lambda_) * P)
        except np.linalg.LinAlgError:
            # 如果Cholesky分解失败，使用特征值分解
            U, s, _ = np.linalg.svd(P)
            sqrt = U @ np.diag(np.sqrt(s * (self.dim_x + self.lambda_)))
        
        # 中心点
        self.sigma_points[:, 0] = x.flatten()
        
        # 正负偏移点
        for k in range(self.dim_x):
            self.sigma_points[:, k+1] = x.flatten() + sqrt[k]
            self.sigma_points[:, k+1+self.dim_x] = x.flatten() - sqrt[k]
            
        return self.sigma_points
        
    def _state_transition(self, x):
        """状态转移函数 - 匀加速运动模型"""
        F = np.array([
            [1, 0, self.dt, 0, 0.5*self.dt**2, 0],
            [0, 1, 0, self.dt, 0, 0.5*self.dt**2],
            [0, 0, 1, 0, self.dt, 0],
            [0, 0, 0, 1, 0, self.dt],
            [0, 0, 0, 0, 1, 0],
            [0, 0, 0, 0, 0, 1]
        ])
        return F @ x
        
    def _measurement_function(self, x):
        """观测函数 - 只观测位置"""
        return x[:2]
        
    def predict(self):
        """预测步骤 - 优化版本"""
        if not self.initialized:
            return None
            
        # 生成Sigma点
        sigma_points = self._generate_sigma_points(self.x, self.P)
        
        # 传播Sigma点通过状态转移函数
        for i in range(self.n_sigma):
            sigma_points[:, i] = self._state_transition(sigma_points[:, i].reshape(-1, 1)).flatten()
        
        # 计算预测均值
        x_pred = np.zeros((self.dim_x, 1))
        for i in range(self.n_sigma):
            x_pred += self.Wm[i] * sigma_points[:, i].reshape(-1, 1)
        
        # 计算预测协方差
        P_pred = self.Q.copy()
        for i in range(self.n_sigma):
            y = sigma_points[:, i].reshape(-1, 1) - x_pred
            P_pred += self.Wc[i] * np.outer(y, y)
        
        self.x = x_pred
        self.P = P_pred
        
        return self.x.copy()
        
    def update(self, measurement):
        """更新步骤 - 优化版本"""
        if not self.initialized:
            self.initialize(measurement)
            return
            
        z = np.array(measurement).reshape((-1, 1))
        
        # 生成Sigma点
        sigma_points = self._generate_sigma_points(self.x, self.P)
        
        # 传播Sigma点通过观测函数
        for i in range(self.n_sigma):
            self.sigma_h[:, i] = self._measurement_function(sigma_points[:, i])
        
        # 预测观测均值
        z_pred = np.zeros((self.dim_z, 1))
        for i in range(self.n_sigma):
            z_pred += self.Wm[i] * self.sigma_h[:, i].reshape(-1, 1)
        
        # 计算协方差矩阵
        Pz = self.R.copy()
        Pxz = np.zeros((self.dim_x, self.dim_z))
        
        for i in range(self.n_sigma):
            z_diff = self.sigma_h[:, i].reshape(-1, 1) - z_pred
            x_diff = sigma_points[:, i].reshape(-1, 1) - self.x
            
            Pz += self.Wc[i] * np.outer(z_diff, z_diff)
            Pxz += self.Wc[i] * np.outer(x_diff, z_diff)
        
        # 卡尔曼增益
        K = Pxz @ np.linalg.inv(Pz)
        
        # 状态更新
        self.x = self.x + K @ (z - z_pred)
        self.P = self.P - K @ Pz @ K.T
        
    def get_state(self):
        """获取当前状态"""
        if not self.initialized:
            return None
        return (float(self.x[0, 0]), float(self.x[1, 0]), 
                float(self.x[2, 0]), float(self.x[3, 0]),
                float(self.x[4, 0]), float(self.x[5, 0]))
                
    def get_position(self):
        """获取当前位置"""
        if not self.initialized:
            return None
        return (float(self.x[0, 0]), float(self.x[1, 0]))
        
    def get_velocity(self):
        """获取当前速度"""
        if not self.initialized:
            return None
        return (float(self.x[2, 0]), float(self.x[3, 0]))
        
    def predict_future(self, steps=3):
        """多步预测 - 预测未来几帧的位置"""
        if not self.initialized:
            return []
            
        future_positions = []
        x_temp = self.x.copy()
        
        for step in range(1, steps + 1):
            # 使用当前状态预测未来位置
            x_future = self._state_transition(x_temp)
            future_positions.append((float(x_future[0]), float(x_future[1])))
            x_temp = x_future.reshape(-1, 1)
            
        return future_positions


class MotionModel:
    """运动模型基类"""
    def __init__(self, dt):
        self.dt = dt
        
    def get_transition_matrix(self):
        """获取状态转移矩阵"""
        raise NotImplementedError
        
    def get_process_noise(self):
        """获取过程噪声矩阵"""
        raise NotImplementedError


class ConstantVelocityModel(MotionModel):
    """匀速运动模型"""
    def get_transition_matrix(self):
        return np.array([
            [1, 0, self.dt, 0, 0, 0],
            [0, 1, 0, self.dt, 0, 0],
            [0, 0, 1, 0, 0, 0],
            [0, 0, 0, 1, 0, 0],
            [0, 0, 0, 0, 0, 0],
            [0, 0, 0, 0, 0, 0]
        ])
        
    def get_process_noise(self):
        return np.diag([0.1, 0.1, 0.5, 0.5, 0.1, 0.1])


class ConstantAccelerationModel(MotionModel):
    """匀加速运动模型"""
    def get_transition_matrix(self):
        return np.array([
            [1, 0, self.dt, 0, 0.5*self.dt**2, 0],
            [0, 1, 0, self.dt, 0, 0.5*self.dt**2],
            [0, 0, 1, 0, self.dt, 0],
            [0, 0, 0, 1, 0, self.dt],
            [0, 0, 0, 0, 1, 0],
            [0, 0, 0, 0, 0, 1]
        ])
        
    def get_process_noise(self):
        return np.diag([0.1, 0.1, 1.0, 1.0, 2.0, 2.0])


class TurningModel(MotionModel):
    """转弯运动模型"""
    def __init__(self, dt, turn_rate=0.1):
        super().__init__(dt)
        self.turn_rate = turn_rate
        
    def get_transition_matrix(self):
        cos_w = np.cos(self.turn_rate * self.dt)
        sin_w = np.sin(self.turn_rate * self.dt)
        
        return np.array([
            [1, 0, sin_w/self.turn_rate, -(1-cos_w)/self.turn_rate, 0, 0],
            [0, 1, (1-cos_w)/self.turn_rate, sin_w/self.turn_rate, 0, 0],
            [0, 0, cos_w, -sin_w, 0, 0],
            [0, 0, sin_w, cos_w, 0, 0],
            [0, 0, 0, 0, 1, 0],
            [0, 0, 0, 0, 0, 1]
        ])
        
    def get_process_noise(self):
        return np.diag([0.2, 0.2, 1.5, 1.5, 1.0, 1.0])


class InteractingMultipleModel:
    """
    交互式多模型滤波器 - 智能运动模式切换
    """
    
    def __init__(self, dt=1.0/30.0):
        """初始化IMM滤波器"""
        self.dt = dt
        
        # 创建多个运动模型
        self.models = [
            ConstantVelocityModel(dt),
            ConstantAccelerationModel(dt),
            TurningModel(dt)
        ]
        self.n_models = len(self.models)
        
        # 为每个模型创建UKF
        self.filters = [UnscentedKalmanFilter(dt) for _ in range(self.n_models)]
        
        # 模型转移概率矩阵 (优化的转移概率)
        self.M = np.array([
            [0.8, 0.15, 0.05],   # 从匀速到其他模型
            [0.2, 0.7, 0.1],     # 从匀加速到其他模型  
            [0.1, 0.1, 0.8]      # 从转弯到其他模型
        ])
        
        # 模型概率
        self.mu = np.array([0.6, 0.3, 0.1])  # 初始偏向匀速模型
        
        # 混合概率
        self.omega = np.zeros((self.n_models, self.n_models))
        
        self.initialized = False
        
    def initialize(self, measurement):
        """初始化所有滤波器"""
        for f in self.filters:
            f.initialize(measurement)
        self.initialized = True
        
    def predict(self):
        """IMM预测步骤"""
        if not self.initialized:
            return None
            
        # 步骤1: 计算混合概率
        c_j = np.dot(self.mu, self.M.T)
        for i in range(self.n_models):
            for j in range(self.n_models):
                self.omega[i, j] = (self.M[i, j] * self.mu[i]) / c_j[j]
        
        # 步骤2: 混合状态估计
        mixed_states = []
        mixed_covs = []
        
        for j in range(self.n_models):
            # 混合均值
            mixed_x = np.zeros((6, 1))
            for i in range(self.n_models):
                if self.filters[i].initialized:
                    mixed_x += self.omega[i, j] * self.filters[i].x
            
            # 混合协方差
            mixed_P = np.zeros((6, 6))
            for i in range(self.n_models):
                if self.filters[i].initialized:
                    y = self.filters[i].x - mixed_x
                    mixed_P += self.omega[i, j] * (self.filters[i].P + np.outer(y, y))
            
            mixed_states.append(mixed_x)
            mixed_covs.append(mixed_P)
        
        # 步骤3: 使用混合初始条件进行预测
        for j in range(self.n_models):
            self.filters[j].x = mixed_states[j]
            self.filters[j].P = mixed_covs[j]
            self.filters[j].predict()
        
        # 更新模型概率
        self.mu = c_j
        
        return self.get_combined_state()
        
    def update(self, measurement):
        """IMM更新步骤"""
        if not self.initialized:
            self.initialize(measurement)
            return
            
        # 更新所有滤波器
        likelihoods = np.zeros(self.n_models)
        
        for j in range(self.n_models):
            self.filters[j].update(measurement)
            
            # 计算似然度 (简化版本)
            z = np.array(measurement).reshape(-1, 1)
            z_pred = self.filters[j]._measurement_function(self.filters[j].x.flatten()).reshape(-1, 1)
            S = self.filters[j].R  # 简化，实际应该包含预测协方差
            
            try:
                residual = z - z_pred
                likelihoods[j] = np.exp(-0.5 * residual.T @ np.linalg.inv(S) @ residual)
            except:
                likelihoods[j] = 1e-10
        
        # 更新模型概率
        if np.sum(likelihoods) > 0:
            self.mu = self.mu * likelihoods
            self.mu = self.mu / np.sum(self.mu)
        
    def get_combined_state(self):
        """获取组合状态估计"""
        if not self.initialized:
            return None
            
        combined_x = np.zeros((6, 1))
        for j in range(self.n_models):
            if self.filters[j].initialized:
                combined_x += self.mu[j] * self.filters[j].x
                
        return combined_x
        
    def get_position(self):
        """获取当前位置"""
        state = self.get_combined_state()
        if state is not None:
            return (float(state[0, 0]), float(state[1, 0]))
        return None
        
    def get_velocity(self):
        """获取当前速度"""
        state = self.get_combined_state()
        if state is not None:
            return (float(state[2, 0]), float(state[3, 0]))
        return None
        
    def get_dominant_model(self):
        """获取主导运动模型"""
        return np.argmax(self.mu)
        
    def get_model_probabilities(self):
        """获取模型概率"""
        return self.mu.copy()


class AdvancedTarget:
    """
    升级版目标类 - 使用UKF或IMM
    """
    
    def __init__(self, target_id: int, bbox: Tuple[int, int, int, int], 
                 class_name: str, confidence: float, use_imm: bool = True):
        """
        初始化目标
        Args:
            use_imm: 是否使用IMM滤波器 (否则使用UKF)
        """
        self.id = target_id
        self.class_name = class_name
        
        # 选择滤波器类型
        if use_imm:
            self.filter = InteractingMultipleModel()
        else:
            self.filter = UnscentedKalmanFilter()
        
        # 计算中心坐标
        cx = (bbox[0] + bbox[2]) / 2.0
        cy = (bbox[1] + bbox[3]) / 2.0
        
        # 初始化滤波器
        self.filter.initialize([cx, cy])
        
        # 目标属性
        self.confidence = confidence
        self.bbox = bbox
        self.last_seen = time.time()
        self.missed_frames = 0
        self.max_missed_frames = 8  # 增加容忍度
        
        # 历史轨迹
        self.trajectory = [(cx, cy)]
        self.max_trajectory_length = 50  # 增加轨迹长度
        
        # 预测历史
        self.prediction_history = []
        self.max_prediction_history = 10
        
        # 颜色
        self.color = self._generate_color(target_id)
        
        # 性能监控
        self.prediction_errors = []
        self.max_error_history = 20
        
    def _generate_color(self, target_id: int) -> Tuple[int, int, int]:
        """为目标生成唯一颜色"""
        np.random.seed(target_id * 123)  # 更好的随机种子
        # 生成更鲜艳的颜色
        hue = (target_id * 137) % 360
        rgb = cv2.cvtColor(np.uint8([[[hue, 255, 255]]]), cv2.COLOR_HSV2RGB)[0, 0]
        return tuple(rgb.tolist())
        
    def predict(self):
        """预测下一帧位置"""
        predicted_state = self.filter.predict()
        if predicted_state is not None:
            pos = self.filter.get_position()
            if pos:
                # 记录预测历史
                self.prediction_history.append(pos)
                if len(self.prediction_history) > self.max_prediction_history:
                    self.prediction_history.pop(0)
            return pos
        return None
        
    def update(self, bbox: Tuple[int, int, int, int], confidence: float):
        """更新目标信息"""
        # 计算新的中心坐标
        cx = (bbox[0] + bbox[2]) / 2.0
        cy = (bbox[1] + bbox[3]) / 2.0
        
        # 计算预测误差 (如果有预测的话)
        if self.prediction_history:
            last_pred = self.prediction_history[-1]
            error = np.sqrt((cx - last_pred[0])**2 + (cy - last_pred[1])**2)
            self.prediction_errors.append(error)
            if len(self.prediction_errors) > self.max_error_history:
                self.prediction_errors.pop(0)
        
        # 更新滤波器
        self.filter.update([cx, cy])
        
        # 更新目标属性
        self.bbox = bbox
        self.confidence = confidence
        self.last_seen = time.time()
        self.missed_frames = 0
        
        # 更新轨迹
        self.trajectory.append((cx, cy))
        if len(self.trajectory) > self.max_trajectory_length:
            self.trajectory.pop(0)
            
    def get_future_predictions(self, steps=3):
        """获取未来多步预测"""
        if hasattr(self.filter, 'predict_future'):
            return self.filter.predict_future(steps)
        else:
            # IMM的多步预测
            predictions = []
            current_pos = self.filter.get_position()
            current_vel = self.filter.get_velocity()
            
            if current_pos and current_vel:
                for step in range(1, steps + 1):
                    dt = step * (1.0/30.0)  # 30 FPS
                    future_x = current_pos[0] + current_vel[0] * dt
                    future_y = current_pos[1] + current_vel[1] * dt
                    predictions.append((future_x, future_y))
                    
            return predictions
            
    def get_prediction_accuracy(self):
        """获取预测精度统计"""
        if not self.prediction_errors:
            return None
        return {
            'mean_error': np.mean(self.prediction_errors),
            'std_error': np.std(self.prediction_errors),
            'max_error': np.max(self.prediction_errors),
            'min_error': np.min(self.prediction_errors)
        }
        
    def get_motion_info(self):
        """获取运动信息"""
        info = {
            'position': self.filter.get_position(),
            'velocity': self.filter.get_velocity(),
        }
        
        # 如果使用IMM，添加模型信息
        if hasattr(self.filter, 'get_dominant_model'):
            model_names = ['ConstantVelocity', 'ConstantAcceleration', 'Turning']
            info['dominant_model'] = model_names[self.filter.get_dominant_model()]
            info['model_probabilities'] = self.filter.get_model_probabilities()
            
        return info
        
    def is_valid(self) -> bool:
        """检查目标是否仍然有效"""
        return self.missed_frames < self.max_missed_frames
        
    def mark_missed(self):
        """标记目标丢失一帧"""
        self.missed_frames += 1


def hungarian_assignment(cost_matrix):
    """
    匈牙利算法进行最优分配
    Args:
        cost_matrix: 代价矩阵
    Returns:
        row_indices, col_indices: 分配结果
    """
    if cost_matrix.size == 0:
        return np.array([]), np.array([])
    
    row_indices, col_indices = linear_sum_assignment(cost_matrix)
    return row_indices, col_indices


class AdvancedMultiTargetTracker:
    """
    升级版多目标跟踪器 - 使用匈牙利算法和马哈拉诺比斯距离
    """
    
    def __init__(self, max_distance_threshold: float = 50.0, use_imm: bool = True):
        """
        初始化高级多目标跟踪器
        Args:
            max_distance_threshold: 最大关联距离阈值
            use_imm: 是否使用IMM滤波器
        """
        self.targets: Dict[int, AdvancedTarget] = {}
        self.next_id = 1
        self.max_distance_threshold = max_distance_threshold
        self.use_imm = use_imm
        
        # 性能监控
        self.frame_count = 0
        self.total_assignment_time = 0.0
        
    def _calculate_mahalanobis_distance(self, pos1: Tuple[float, float], 
                                      pos2: Tuple[float, float], 
                                      covariance: np.ndarray = None) -> float:
        """
        计算马哈拉诺比斯距离
        """
        if covariance is None:
            # 如果没有协方差矩阵，使用欧几里得距离
            return np.sqrt((pos1[0] - pos2[0])**2 + (pos1[1] - pos2[1])**2)
            
        diff = np.array([pos1[0] - pos2[0], pos1[1] - pos2[1]])
        try:
            distance = np.sqrt(diff.T @ np.linalg.inv(covariance) @ diff)
            return distance
        except:
            # 如果协方差矩阵奇异，回退到欧几里得距离
            return np.sqrt(diff.T @ diff)
            
    def _build_cost_matrix(self, detections: List[Tuple]) -> Tuple[np.ndarray, List, List]:
        """
        构建代价矩阵用于匈牙利算法
        """
        if not self.targets or not detections:
            return np.array([]), [], []
            
        # 获取所有目标的预测位置
        target_positions = []
        target_ids = []
        for target_id, target in self.targets.items():
            pred_pos = target.predict()
            if pred_pos:
                target_positions.append(pred_pos)
                target_ids.append(target_id)
                
        if not target_positions:
            return np.array([]), [], []
            
        # 计算检测结果的中心位置
        detection_positions = []
        for bbox, _, _ in detections:
            cx = (bbox[0] + bbox[2]) / 2.0
            cy = (bbox[1] + bbox[3]) / 2.0
            detection_positions.append((cx, cy))
            
        # 构建代价矩阵
        n_targets = len(target_positions)
        n_detections = len(detection_positions)
        cost_matrix = np.full((n_targets, n_detections), self.max_distance_threshold * 2)
        
        for i, target_pos in enumerate(target_positions):
            for j, det_pos in enumerate(detection_positions):
                distance = self._calculate_mahalanobis_distance(target_pos, det_pos)
                if distance < self.max_distance_threshold:
                    cost_matrix[i, j] = distance
                    
        return cost_matrix, target_ids, detection_positions
        
    def update(self, detections: List[Tuple]):
        """
        更新跟踪器 - 使用匈牙利算法优化分配
        """
        start_time = time.time()
        self.frame_count += 1
        
        # 构建代价矩阵
        cost_matrix, target_ids, detection_positions = self._build_cost_matrix(detections)
        
        associations = {}
        used_detections = set()
        
        if cost_matrix.size > 0:
            # 使用匈牙利算法进行最优分配
            row_indices, col_indices = hungarian_assignment(cost_matrix)
            
            # 过滤掉代价过高的分配
            for row, col in zip(row_indices, col_indices):
                if cost_matrix[row, col] < self.max_distance_threshold:
                    target_id = target_ids[row]
                    associations[target_id] = col
                    used_detections.add(col)
        
        # 更新关联的目标
        for target_id, detection_idx in associations.items():
            bbox, class_name, confidence = detections[detection_idx]
            self.targets[target_id].update(bbox, confidence)
            
        # 标记未关联的目标为丢失
        for target_id in list(self.targets.keys()):
            if target_id not in associations:
                self.targets[target_id].mark_missed()
                # 删除无效目标
                if not self.targets[target_id].is_valid():
                    del self.targets[target_id]
                    
        # 为未关联的检测创建新目标
        for i, (bbox, class_name, confidence) in enumerate(detections):
            if i not in used_detections:
                new_target = AdvancedTarget(self.next_id, bbox, class_name, 
                                          confidence, use_imm=self.use_imm)
                self.targets[self.next_id] = new_target
                self.next_id += 1
                
        # 性能统计
        assignment_time = time.time() - start_time
        self.total_assignment_time += assignment_time
        
        return list(self.targets.values())
        
    def get_performance_stats(self):
        """获取性能统计信息"""
        if self.frame_count == 0:
            return {}
            
        return {
            'avg_assignment_time': self.total_assignment_time / self.frame_count * 1000,  # ms
            'total_frames': self.frame_count,
            'active_targets': len(self.targets),
            'next_id': self.next_id - 1
        }


def draw_advanced_tracking_results(image, targets: List[AdvancedTarget], 
                                 show_trajectory: bool = True,
                                 show_prediction: bool = True,
                                 show_future_predictions: bool = True,
                                 show_model_info: bool = True):
    """
    绘制升级版跟踪结果
    """
    for target in targets:
        bbox = target.bbox
        color = target.color
        
        # 绘制检测框
        cv2.rectangle(image, (int(bbox[0]), int(bbox[1])), 
                     (int(bbox[2]), int(bbox[3])), color, 2)
        
        # 目标信息
        motion_info = target.get_motion_info()
        pos = motion_info.get('position')
        vel = motion_info.get('velocity')
        
        if pos and vel:
            speed = np.sqrt(vel[0]**2 + vel[1]**2)
            
            # 主标签
            main_label = f"ID:{target.id} {target.class_name} {target.confidence:.2f}"
            cv2.putText(image, main_label, (int(bbox[0]), int(bbox[1]) - 10),
                       cv2.FONT_HERSHEY_SIMPLEX, 0.5, color, 2)
            
            # 详细信息
            detail_label = f"({pos[0]:.0f},{pos[1]:.0f}) v:{speed:.1f}"
            cv2.putText(image, detail_label, (int(bbox[0]), int(bbox[1]) - 30),
                       cv2.FONT_HERSHEY_SIMPLEX, 0.4, color, 1)
            
            # 模型信息 (如果使用IMM)
            if show_model_info and 'dominant_model' in motion_info:
                model_label = f"Model: {motion_info['dominant_model'][:3]}"
                cv2.putText(image, model_label, (int(bbox[0]), int(bbox[1]) - 50),
                           cv2.FONT_HERSHEY_SIMPLEX, 0.3, color, 1)
            
            # 当前位置点
            cv2.circle(image, (int(pos[0]), int(pos[1])), 6, color, -1)
            
            # 绘制轨迹
            if show_trajectory and len(target.trajectory) > 1:
                trajectory_points = [(int(x), int(y)) for x, y in target.trajectory]
                for i in range(1, len(trajectory_points)):
                    # 渐变效果
                    alpha = i / len(trajectory_points)
                    thickness = max(1, int(alpha * 3))
                    cv2.line(image, trajectory_points[i-1], trajectory_points[i], 
                            color, thickness)
            
            # 绘制即时预测
            if show_prediction:
                pred_pos = target.predict()
                if pred_pos and pos:
                    cv2.circle(image, (int(pred_pos[0]), int(pred_pos[1])), 
                              4, (0, 255, 255), -1)  # 黄色预测点
                    # 预测向量
                    cv2.arrowedLine(image, (int(pos[0]), int(pos[1])),
                                   (int(pred_pos[0]), int(pred_pos[1])), 
                                   (0, 255, 255), 2)
            
            # 绘制未来预测
            if show_future_predictions:
                future_preds = target.get_future_predictions(5)
                if future_preds:
                    for i, future_pos in enumerate(future_preds):
                        alpha = 255 - i * 40  # 逐渐透明
                        color_faded = (0, alpha, alpha)
                        cv2.circle(image, (int(future_pos[0]), int(future_pos[1])), 
                                  2, color_faded, -1)
                        
                        # 连接线
                        if i == 0 and pos:
                            start_pos = pos
                        else:
                            start_pos = future_preds[i-1]
                        cv2.line(image, (int(start_pos[0]), int(start_pos[1])),
                                (int(future_pos[0]), int(future_pos[1])), 
                                color_faded, 1)
    
    return image 